/**************************************************************************** 
 * Meta，一个轻量级的静态分区Hypervisor 
 *
 * 版权(c) , 2022-
 *
 * 作者:
 *      Tupelo Shen <shenwanjiang2013@163.com>
 *
 * Meta是一个自由软件，你可以在遵循GNU GPLv2协议的条件下，重新修改并发布它。
 *
 ***************************************************************************/

#include <arch/meta.h>
#include <arch/sysregs.h>
#include <arch/page_table.h>
#include <asm_defs.h>

/**
 * 该段位于`.data`段，按照`2^3=8`字节对齐
 * 在引导阶段提供最小化的同步手段（类似于屏障指令）-其它Core等待bsp设置它
 * 那么调用标签_barrier的地方就是填充8个字节的`0`
 */
.data 
.align 3
_barrier: .8byte 0

/**
 * 下面的代码必须在镜像文件的开始，因为这是该程序的入口点。
 * 因此，`.boot`段必须位于链接脚本的第一个段中。
 * 不要在`_reset_handler`函数之前实现任何代码。
 * "ax": ax <=> `allocation execute`，代表分配可执行
 */
.section ".boot", "ax"
.globl _reset_handler
.globl _el2_entry
_el2_entry:
_reset_handler:

    /**
     * （1）检查ID寄存器，保证支持VE和TZ，4K内存颗粒度和其它需要的特性。
     * （2）检查当前异常级别（EL）。当然，我们期望运行在EL2。
     * （3）该boot代码不遵循任何ABI约定。也就是对于寄存器的使用有自己的约定：
     *      a). x0 -> 传递参数cpu id
     *      b). x1 -> 传递镜像(image)的加载基地址
     *      c). x2 -> 传递config二进制文件加载地址（通过x0传递，比如u-boot传递）
     */
    mov     x2, x0
    mrs     x0, MPIDR_EL1
    adrp    x1, _image_start

    /* 
     * 设置异常向量表（以防在初始化阶段发生异常）
     */
    adr x3, _hyp_vector_table
    msr VBAR_EL2, x3

    /** 根据cluster簇和簇内CPU核的数量，线性化cpu id
     *  我们只考虑2级亲和力
     *  TODO: 这可以通过其它方式完成。我们不应该在初始化早期阶段依赖平台描述。
     */
    mov x3, x0, lsr #8 
    and x3, x3, 0xff
    adr x4, platform
    /* PLAT_ARCH_OFF+PLAT_ARCH_CLUSTERS_OFF+PLAT_CLUSTERS_CORES_NUM_OFF
     * 该偏移量正好是platform数据结构中存放核数量的位置
     */
    ldr x4, [x4, PLAT_ARCH_OFF+PLAT_ARCH_CLUSTERS_OFF+PLAT_CLUSTERS_CORES_NUM_OFF]
    ldr x5, =META_VAS_BASE
    sub x4, x4, x5      /* x4存放CPU核内存地址到虚拟地址空间基地址的偏移量 */
    add x4, x4, x1      /* x1存放的是代码的入口地址（物理） */
    mov x5, xzr         /* x5清零 */
    mov x7, xzr         /* x7清零 */
1:
    cmp x5, x3          /* x5累加cluster数，如果是cluster 0，则跳转到2 */
    b.ge    2f          
    ldrb w6, [x4]       /* 将核数量写入w6 */
    add x4, x4, #1      /* 取下一个cluster的核数量 */
    add x5, x5, #1      /* cluster+1 */
    add x7, x7, x6      /* x7记录上一个cluster核数 */
    b   1b
2:
    and x0, x0, #0xff
    add x0, x0, x7      /* 最后，每个核都获得了一个自然数编码的号 */

/* 添加8个字节的0，作为全局标志使用 */
.pushsection .data
_master_set:
    .8byte  0
.popsection

/**
 * 为了设置主核，我们假设最初只有一个核被激活，然后由它激活其它核。
 * 因此，当设置CPU_MASTER时，不存在并发性问题，不需要原子操作。
 *
 * 如果判断到上面的8个字节为0，则是第一个执行这段代码的核，设为主核。
 * 后执行的核在读取这8个字节时，发现不等于0，跳过设置主核的代码，
 * 跳转到前面执行。
 */

_set_master_cpu:
    adr x3, _master_set
    ldr x9, [x3]
    cbnz x9, 7f
    mov x9, #1
    str x9, [x3]
    mov x9, xzr
    adr x3, CPU_MASTER
    str x0, [x3]
7: 

    /** 
     * TODO: bring the system to a well known state. This includes disabling 
     * the MMU (done), all caches (missing i$), BP and others...
     * and invalidating them.   
     */
 
    /* 清除堆栈指针SP，避免在boot阶段发生非对齐SP异常 */
    mov x3, xzr
    mov sp, x3

    /* 禁止cache和MMU */
    mrs x3, SCTLR_EL2 
    bic x3, x3, #0x7
    msr SCTLR_EL2, x3 

    /* 失效cache */
    /* 我们应该清除吗？*/
    mov x19, x0
    mov x20, x1
    mov x21, x2

    mov x0, #0
    bl  cache_invalidate
    cbnz x9, 1f             /* 主核失效L2-Cache即可 */
    mov x0, #2
    bl  cache_invalidate
1:
    mov x0, x19
    mov x1, x20
    mov x2, x21

    ic iallu                /* invalidate all instruction caches to PoU */

    /* Skip initialy global page tables setup if not bsp (boot cpu) */
    cbnz    x9, wait_for_bsp

    /* 对存储全局页表的内存区域进行清零 */
    adr x16, _page_tables_start 
    adr x17, _page_tables_end   
    bl  clear               

    /* 设置临时线性映射 
     * 1. 先确定镜像的入口地址在临时线性映射中的索引
     * 2. 入口地址添加PTE属性（如该项指向的超级块）
     * 3. 将入口地址存入root_l1_flat_pt相应位置上（根据索引）
     */
    adr x4, root_l1_flat_pt     /* root_l1_flat_pt物理地址 */
    PTE_INDEX_ASM x5, x1, 1 
    add x6, x1, #(PTE_HYP_FLAGS | PTE_SUPERPAGE)
    str x6, [x4, x5]
    
    /* Set global root mappings for hypervisor image */
    /* 为hypervisor image设置全局root映射 */
    /*
     * root_l1_flat_pt
     *      ↓           → 临时线性映射
     * image_start
     * --------------------------------------------------------------------
     * root_l1_pt(1级页表)    (2级页表)          (3级页表)       (物理内存)
     *      ↓
     * -------------        -------------        ---------        ________
     * | root_l2_pt | ----→ | root_l3_pt | ----→ | page0 | ----→ |        |
     * -------------        -------------        ---------       | image  |
     * |  ...   |                 ...            | page1 | ----→ |        |
     * ----------                                ---------       |________|
     * |        |                                   ...          |        |
     * ----------                                | page63|       | others |
     *                                                           |........|
     */
    adr x4, root_l1_pt
    ldr x5, =(PTE_INDEX(1, META_VAS_BASE)*8)
    adr x6, root_l2_pt
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]

    adr x4, root_l2_pt
    ldr x5, =(PTE_INDEX(2, META_VAS_BASE)*8)
    adr x6, root_l3_pt
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]

    adr x4, root_l3_pt
    ldr x7, =_image_start
    ldr x8, =_image_end
    adr x6, _image_start
    add x6, x6, #(PTE_HYP_FLAGS | PTE_PAGE)
1:
    /* 循环将镜像的物理page基地址写入到root_l3_pt页表中 */
    cmp x7, x8
    b.ge 2f
    //lsr x5, x7, #(PTE_INDEX_SHIFT(3)-3)
    PTE_INDEX_ASM x5, x7, 3
    str x6, [x4, x5]
    add x6, x6, #PAGE_SIZE
    add x7, x7, #PAGE_SIZE
    b   1b
2:
    adr x5, _barrier
    mov x4, #1
    str x4, [x5]
    //  dsb // arguments?
    sev
    b   map_cpu

/* 等待bootstrap（bsp）完成全局页表映射 */
wait_for_bsp:   
    wfe
    ldr x4, _barrier
    cmp x4, #1
    b.lt wait_for_bsp

map_cpu:
    /**
     *    x3 -> cpu base phys
     *    x4 -> current pt base phys
     *    x5 -> pte index
     *    x6 -> phys addr
     *    x7 -> virt addr
     *    x8 -> aux 
     */

    /* 获取cpu root pt、
     * cpu数据结构的大小为`CPU_SIZE + (PT_SIZE*(PT_LVLS-1))`
     */
    mov x8, #(CPU_SIZE + (PT_SIZE*(PT_LVLS-1)))
    adrp x3, _dmem_phys_beg
    madd x3, x0, x8, x3     /* cpu_num * (cpu_size + root_pt_size) + dmem_base_addr */
    
    /* 清零存放cpu数据结构的内存 */
    mov x16, x3 
    add x17, x3, x8
    bl  clear   

    /* 获取cpu结构中的root pt的指针 */
    add x4, x3, #CPU_ROOT_PT_OFF

    /* 映射最初的bootstrap平面映射 */
    PTE_INDEX_ASM x5, x1, 0         /* x1 - 程序入口地址（物理） */
    adr x6, root_l1_flat_pt         
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]
    
    ldr x5, =(PTE_INDEX(0, META_VAS_BASE)*8)
    adr x6, root_l1_pt
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]

    ldr x5, =(PTE_INDEX(0, META_CPU_BASE)*8)
    //add x6, x4, #PT_SIZE
    add x6, x3, #CPU_SIZE
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]

    //add x4, x4, #PT_SIZE
    add x4, x3, #CPU_SIZE
    ldr x5, =(PTE_INDEX(1, META_CPU_BASE)*8)
    add x6, x4, #PT_SIZE
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]

    add x4, x4, #PT_SIZE
    ldr x5, =(PTE_INDEX(2, META_CPU_BASE)*8)
    add x6, x4, #PT_SIZE
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x6, [x4, x5]

    add x4, x4, #PT_SIZE
    ldr x7, =META_CPU_BASE
    add x8, x7, #CPU_SIZE
    mov x6, x3
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)
1:
    cmp x7, x8
    b.ge map_cpu_interface
    PTE_INDEX_ASM x5, x7, 3
    str x6, [x4, x5]
    add x6, x6, #PAGE_SIZE
    add x7, x7, #PAGE_SIZE
    b   1b

map_cpu_interface:

    adr x4, root_l3_pt

    add x6, x3, #CPU_IF_OFF
    add x6, x6, #(PTE_HYP_FLAGS | PTE_TABLE)

    mov x8, #CPU_IF_SIZE
    ldr x7, =_dmem_beg
    madd x7, x0, x8, x7
    add x8, x8, x7

1:
    cmp x7, x8
    b.ge setup_cpu
    PTE_INDEX_ASM x5, x7, 3 
    str x6, [x4, x5]

    add x7, x7, #PAGE_SIZE
    add x6, x6, #PAGE_SIZE

    b 1b

setup_cpu:

    /**
     *  The operation is purposely commented out.
     *  We are assuming monitor code already enabled smp coherency.
     */ 
    /* Turn on smp coherence */
    //mrs   x3, CPUECTLR_EL1    
    //orr   x3, x3, #(CPUECTLR_SPEN_BIT)
    //msr   CPUECTLR_EL1, x3 

    /* 架构特性陷阱寄存器 */
    mov x3, xzr
    msr CPTR_EL2, x3

    /* 设置地址转换配置 */
    ldr x3, =TCR_EL2_DFLT
    msr TCR_EL2, x3

    /* 设置hypervisor默认内存属性 */
    ldr x3, =MAIR_EL2_DFLT
    msr MAIR_EL2, x3

    /* 将CPU核自己的根页表的基地址写入到TTBR0_EL2*/
    mov x8, #(CPU_SIZE + (PT_SIZE*(PT_LVLS-1)))
    adrp x3, _dmem_phys_beg
    madd x3, x0, x8, x3
    add x3, x3, #CPU_ROOT_PT_OFF
    msr TTBR0_EL2, x3

    /* 配置CPU核的堆栈 */
    mov x3, #(SPSel_SP)                             
    msr SPSEL, x3   
    ldr x3, =cpu
    add x3, x3, #(CPU_STACK_OFF + CPU_STACK_SIZE)
    mov SP, x3

    /** 
     * TODO: set implementation defined registers such as ACTLR or AMAIR.
     * Maybe define a macro for this in a implementation oriented directory
     * inside arch.
     */

    /**
     * TODO: invalidate caches, TLBs and branch prediction.
     * Need for barriers?
     */

    ldr x5, =_enter_vas

    /* 使能MMU和cache；
     * 使能MMU之后，此时已经使用虚拟地址到物理地址的转换功能。
     * 但是，此时PC寄存器的值还是原先的物理地址，没有更新到虚拟地址。
     * 为了平滑过渡到虚拟地址空间，必须建立平面映射
     */
    ldr x4, =(SCTLR_RES1 | SCTLR_M | SCTLR_C | SCTLR_I)
    msr SCTLR_EL2, x4
    
    /* 失效TLB */
    tlbi    alle2
    dsb nsh
    isb
    
    br  x5

_enter_vas:

    /* 将异常向量表的虚拟地址写入到VBAR_EL2寄存器 */
    ldr x3, =_hyp_vector_table
    msr VBAR_EL2, x3

    /* 删除临时映射 - the L1 page holding it leaks */
    ldr x4, =cpu
    add x4, x4, #CPU_ROOT_PT_OFF
    PTE_INDEX_ASM x5, x1, 0 
    str xzr, [x4, x5]

    tlbi    alle2
    dsb nsh
    isb

    /* 如果是主核（CPU 0），则清零bss */
    cbnz x9, 1f
    ldr x16, =_bss_start    
    ldr x17, =_bss_end  
    bl  clear   

    adr x5, _barrier
    mov x4, #2
    str x4, [x5]    

1:
    /* 等待主核完成清零bss段 */
    ldr x4, _barrier
    cmp x4, #2
    b.lt 1b

    isb

    /* 进入C代码的世界 */
    b init

    /* This point should never be reached */
    b   .               

/*
 * Helper functions for boot code.
 * 对一段内存进行清零操作
 */
.func clear
clear:
2:
    cmp x16, x17            
    b.ge 1f             
    str xzr, [x16], #8  
    b   2b              
1:
    ret
.endfunc

/*
 * 代码取自"Application Note Bare-metal Boot Code for ARMv8-A
 * Processors - Version 1.0"
 *
 * x0 - 传递要失效的cache等级 (0: L1 D-Cache, 1: L1 I-Cache, 2: L2 - 统一Cache)
 */

.func cache_invalidate
cache_invalidate:
    msr csselr_el1, x0      /* read Cache Size Select register */
    mrs x4, ccsidr_el1      /* read Cache Size Id */
    and x1, x4, #0x7        /* get LineSize field */
    add x1, x1, #0x4        /* x1 = cache line size */
    ldr x3, =0x7fff
    and x2, x3, x4, lsr #13 /* x2 = cache set number – 1 */
    ldr x3, =0x3ff
    and x3, x3, x4, lsr #3  /* x3 = cache associativity number – 1 */
    clz w4, w3              /* x4 = way position in the cisw instruction */
    mov x5, #0              /* x5 = way counter way_loop */
way_loop:
    mov x6, #0              // x6 = set counter set_loop.
set_loop:
    lsl x7, x5, x4
    orr x7, x0, x7          // set way.
    lsl x8, x6, x1
    orr x7, x7, x8          // set set.
    dc cisw, x7             // clean and invalidate cache line.
    add x6, x6, #1          // increment set counter.
    cmp x6, x2              // last set reached yet?
    ble set_loop            // if not, iterate set_loop,
    add x5, x5, #1          // else, next way.
    cmp x5, x3              // last way reached yet?
    ble way_loop            // if not, iterate way_loop
    ret
.endfunc


.global psci_boot_entry
.func psci_boot_entry
psci_boot_entry:
warm_boot:

    adr x3, _hyp_vector_table
    msr VBAR_EL2, x3

    /* save x0 which contains pointer to saved state psci context */
    mov x19, x0
        /* invalidate l1$ */
    mov x0, #0
    bl  cache_invalidate

    /* restore all needed register state */
    ldp x5, x6, [x19, #0]
    msr TCR_EL2, x5
    msr TTBR0_EL2, x6
    ldp x5, x6, [x19, #16]
    msr MAIR_EL2, x5
    msr CPTR_EL2, x6
    ldp x5, x6, [x19, #32]
    msr HCR_EL2, x5
    msr VMPIDR_EL2, x6
    ldp x5, x6, [x19, #48]
    msr VTCR_EL2, x5
    msr VTTBR_EL2, x6
    ldp x0, x5, [x19, #64]   /* wake up reason is the arg of the later psci_wake call */

    /* map original bootstrap flat mappings */
    mrs x3, TTBR0_EL2
    adrp x1, _image_start
    PTE_INDEX_ASM x1, x1, 0 
    add x3, x3, x1
    dc civac, x3  //we invalidated l1$, but make sure the pte is not in l2$
    add x5, x5, #(PTE_HYP_FLAGS | PTE_TABLE)
    str x5, [x3]

    /* Install vector table virtual address*/
    ldr x3, =_hyp_vector_table
    msr VBAR_EL2, x3

    tlbi    alle2 
    dsb nsh
    isb

    /* Enable MMU and caches */
    ldr x4, =(SCTLR_RES1 | SCTLR_M | SCTLR_C | SCTLR_I)
    msr SCTLR_EL2, x4

    dsb nsh
    isb
    
    ldr x5, =_enter_vas_warm
    br  x5      

_enter_vas_warm:
    /* Unmap bootstrat flat mappings */
    ldr x4, =cpu
    add x3, x4, #(CPU_STACK_OFF+CPU_STACK_SIZE)

    add x4, x4, #CPU_ROOT_PT_OFF
    PTE_INDEX_ASM x5, x1, 0 
    str xzr, [x4, x5]
    tlbi    alle2
    dsb nsh
    isb

    /* Initialize stack pointer */
    mov sp, x3

    bl  psci_wake
    b   .

.endfunc

